/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/*************************************************************************
*  Sickmapf2LSQ.c
*
* Calculates match between reference scan and measured scan.
* Main function is sickmap
* MatchPairPoints does LSQ estimation for the best fit. 
*
* GLOBALS:   NNNX,NNNY,NNNA number of grids (particles) in X,Y,Angle direction
*            xdd, ydd, add Size of grids
*            ddd: longest distance to execepted between closest neighbours  
*
* Author: Jari
* Last Edited: 8.10.2003
* Acknowledgements: Janne on Guru
*****************************************************************************/


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <malloc.h>
#include <time.h>
#include "sickmapf2LSQ.h"
#include "scanMatch.h"

//#include <mex.h>
//#include "matrix.h"

// NUMBER OF PARTICLES in X, Y and andle direction. in total computation (NNX*NNY*NNA)	



void hpsrt(float *val,int * indx, int N);
/*mex -v sickmapf2.c*/

static int NNNX = 8;
static int NNNY = 10;
static int NNNA = 10;

static float add=(float)(.6/57.29578);
static float xdd=(float)0.06;
static float ydd=(float)0.06;
static float ddd=(float)0.06;


void setNumberOfParticles(int NX,int NY, int NA){
	NNNX=NX;
	NNNY=NY;
	NNNA=NA;
}

void setGridSize(float ad, float xd, float yd, float dd){
	add=ad/57.29578;
	xdd=xd;
	ydd=yd;
	ddd=dd;
}





//*********************************************************************

void MatchPairPoints(TFPoint RefPoints[], TFPoint ActualPoints[], int N,
                     float *Tx, float *Ty, float *fi)

{
  float x=0,y=0,xr=0,yr=0;
  float Sum_xx=0,Sum_yy=0,Sum_xy=0,Sum_yx=0;
  int i;

  if (N<3) {
    //fprintf(stderr,"\n\n Low number of points \n\n");
  }
  
  for(i=0; i<N; i++) {
    xr += RefPoints[i].X;
    yr += RefPoints[i].Y;
    x  += ActualPoints[i].X;
    y  += ActualPoints[i].Y;
  }

  x = x/(float)N;   y = y/(float)N;
  xr= xr/(float)N;  yr= yr/(float)N;

  for(i=0; i<N; i++) {
    Sum_xx += (RefPoints[i].X - xr) * (ActualPoints[i].X - x);
    Sum_yy += (RefPoints[i].Y - yr) * (ActualPoints[i].Y - y);
    Sum_xy += (RefPoints[i].X - xr) * (ActualPoints[i].Y - y);
    Sum_yx += (RefPoints[i].Y - yr) * (ActualPoints[i].X - x);
  }

  *fi = atan((Sum_xy-Sum_yx)/(Sum_xx+Sum_yy));
  *Tx = x - (xr*cos(*fi)-yr*sin(*fi));
  *Ty = y - (xr*sin(*fi)+yr*cos(*fi));
}



/**************SICKMAP*************************************************/
/*
* Callculates match between two scans
* add is angle step
* xdd is step in x direction
* ydd is step in y direction
* ddd is limit for excepting the closest neighbour
* 
* units are in meters! 
* TODO: Muisti varaus, vapautus, heap sort 2 qsort? 
* 
***********************************************************************************/

int sickmap(float *a, float *r,float *ix2, float *iy, float *v){

	int m=0,j,i,n,p,q,I,M,N,indx[1024],ia,ib,ic,numa;
	int hit,hit0;
	float iy0,ix0,ad,xd,yd, *sinlut, *coslut;
	float *X, *g, *ix;//[4][15*15*15];
	float LeftLimitForAngle;
	float LeftLimitForX;
	float LeftLimitForY;
	float lsqX=0,lsqY=0,lsqA=0;
	int counter=0;
	int smallestHit=0;
	int smallestHitIndex=0;
	//int smallestI;
	//float angleNum;
	float GyroAngle;
	TFPoint ref[361],act[361];



	GyroAngle=(v[2])*(float)57.0;

	//if(GyroAngle<10.0) angleNum=0;
	//else angleNum=(v[2])*57.0/3.0; 

	numa=NNNA;// + abs(angleNum);

	LeftLimitForAngle = v[2]-add*(numa/2);
	LeftLimitForX = v[0]-(NNNX/2)*xdd;
	LeftLimitForY= v[1]-(NNNY/2)*ydd; //FOR FORWARD WALKING
	

	//M=mxGetNumberOfElements(mr); // M=number of elements in act scan
	//N=mxGetNumberOfElements(mix); // N=nuber of elements in ref scan 
	
	//mslut=zeros(numa*M,1);
	//mclut=zeros(numa*M,1);
	//mX=zeros(3,3);
	//mg=zeros(3,1);
	
	//mx2=zeros(N+1,1);

//	my=zeros(M,1);
//	x=mxGetPr(mx);
//	y=mxGetPr(my);
	
	M = (int) _msize(r) / sizeof(float);
	N = (int) _msize(ix2) / sizeof(float);

	//fprintf(stderr,"ACT: %d : REF: %d\n",M,N);

	//sinlut=mxGetPr(mslut);
	//coslut=mxGetPr(mclut);
	
	sinlut=(float *) malloc(M*numa*sizeof(float));
	coslut=(float *) malloc(M*numa*sizeof(float));

	//X=mxGetPr(mX);
	X=(float *) malloc(9*sizeof(float));

	//g=mxGetPr(mg);
	g=(float *) malloc(3*sizeof(float));
	
	//ix=mxGetPr(mx2);
	//ix = 

	//ix2=mxGetPr(mix);
	
	ix= (float * ) malloc((N+1) * sizeof(float));

	//iy=mxGetPr(miy);
	


	
	
	for(m=0;m<N;m++)
	{
	    ix[m]=ix2[m];
        indx[m]=m;
    }
    ix[N]=1000000.;
	hpsrt(ix,indx,N);
	
	hit0=0;
   
	/*Cycle through x,y,a ...*/
	
	for(ad=LeftLimitForAngle,I=0,i=0;i<numa;i++,I+=M)
	{
		ad+=add;
		for(j=0;j<M;j++)
		{
		    coslut[I+j]=cos(a[j]+ad);
			sinlut[I+j]=sin(a[j]+ad);
		}
	}
	
    for(ad=LeftLimitForAngle,I=0,n=0;n<numa;n++,I+=M)
	{
		ad+=add;
		xd=LeftLimitForX;
		for(p=0;p<NNNX;p++)
		{
			xd+=xdd;
			yd=LeftLimitForY;
			for(q=0;q<NNNY;q++)
			{
				yd+=ydd;
				hit=0;
				
				/*and calculate number of matches;complexity is here M*log2(m) */
				for(j=0;j<M;j++)
				{
				    
					if (r[j]>100.);
					else
					{
						ix0=r[j]*coslut[I+j]+xd-ddd;
					    iy0=r[j]*sinlut[I+j]+yd;
						
						ia=-1;
						ib=N;
					
						if(((ix[0]+xdd)>ix0)||(ix0>ix[N-1]));
						else
						{
							
							while((ib-ia)>1)
							{
								ic=(ia+ib)>>1;
								if(ix0<=ix[ic])
									ib=ic;
								else
							        ia=ic;
							}/*while*/
						    if(ia==-1)
						        ic=0;
						    else
						        ic=ia+1;
						        
							ix0=ix0+ddd;
							while((ix[ic]-ix0)<ddd)
							{
							    if(fabs(iy0-iy[indx[ic]])<ddd)
								{
								    ref[(int)hit].X=ix[ic];
									ref[(int)hit].Y=iy[indx[ic]];
									act[(int)hit].X=ix0;
									act[(int)hit].Y=iy0;
									hit++;
								    break;
							    }
							    ic++;
							}
						}/*else*/
					}/*else*/
						
				}/*for j*/
					

	
	
	
	/*************************
				if(counter==0) smallestHit=hit;

				if(counter<APOSDISTRI){
					if(hit<smallestHit){
						smallestHit=hit;
						smallestHitIndex=counter;
					}
					allX[counter]=xd;
					allY[counter]=yd;
					allPhii[counter]=ad;
					allHits[counter]=hit;
				}
				else if(hit>smallestHit){
					allX[smallestHitIndex]=xd;
					allY[smallestHitIndex]=yd;
					allPhii[smallestHitIndex]=ad;
					allHits[smallestHitIndex]=hit;
					
					smallestHit=allHits[0];
					smallestHitIndex=0;
					for(smallestI=1;smallestI<APOSDISTRI;smallestI++){
						if(allHits[smallestI]<smallestHit){
							smallestHit=allHits[smallestI];
							smallestHitIndex=smallestI;
						}//for
					}//if
				}//elseif
					
				counter++;

  ********************************************************/
				
				if (hit>hit0)
				{
					if(hit>50){
					MatchPairPoints(ref, act, (hit-1), &lsqX, &lsqY, &lsqA);
					}
					else{
						lsqA=0;
						lsqX=0;
						lsqY=0;
					}
					v[2]=ad-lsqA;
					v[0]=xd-lsqX;
					v[1]=yd-lsqY;
					hit0=hit;
				}
			}/* q */
			
		}/* p */
		
	} /* n */

	free(sinlut);
	free(coslut);
	free(X);
	free(g);
	free(ix);

return hit0;
}



void hpsrt(float *ra,int * indx, int n)
{
    unsigned long i,ir,j,k,ira;
    float rra;
    if (n < 1) return;
    k=(n >> 1);
    ir=n-1;
    for (;;) 
    {
        if(k > 0)
        { 
            rra=ra[--k];
            ira=indx[k];
        }
        else 
        {
            rra=ra[ir];
            ira=indx[ir];
            ra[ir]=ra[0];
            indx[ir]=indx[0];
            if (--ir == 0) 
            {
                ra[0]=rra; 
                indx[0]=ira; 
                break;
            }
        }
        i=k;
        j=k+1;
        while (j <= ir)
        {
            if (j < ir && ra[j] < ra[j+1]) j++;
            if (rra < ra[j])
            {
                ra[i]=ra[j];
                indx[i]=indx[j];  
                i=j;
                j <<= 1;
            } else break;
        }
        ra[i]=rra;
        indx[i]=ira;
    }
}







/*****************************************************'
int main(){
	float *pos;
	float *r,*a;
	float *x,*y;
	int i;
	clock_t t1,t2;
	float time;
	int hit;

	pos=(float *) malloc(3*sizeof(float));
	r=(float *) malloc(361*sizeof(float));
	a=(float *) malloc(361*sizeof(float));
	x=(float *) malloc(361*sizeof(float));
	y=(float *) malloc(361*sizeof(float));


	pos[0]=0; pos[1]=0; pos[2]=0;

	for(i=0;i<361;i++){
		r[i]=10;
		a[i]=i*0.5*PI/180;
		x[i]=((float) rand() /RAND_MAX)*20.0;
		y[i]=((float) rand() /RAND_MAX)*20.0;
	}
	t1=clock();
	hit=sickmap(a,r,x,y,pos);
	t2=clock();
	
	time=(float)(t2-t1)/CLOCKS_PER_SEC;

	fprintf(stderr,"%POS: %lf, %lf, %lf: HIT:%d Time %lf\n",pos[0],pos[1],pos[2],hit,time);

	free(pos);
	free(a);
	free(r);
	free(x);
	free(y);

}
*****************************************************************/